#ifndef __PCPT_COMM_TF_H__
#define __PCPT_COMM_TF_H__

#include "comm/comm_base.h"
#include "comm_extend/comm_base_extend.h"

#include <pcl/common/transforms.h>
#include <pcl_ros/transforms.h>
#include <tf/transform_listener.h>

using namespace std;

/**************************************************************************************
功能描述: 位姿变换
           fx = coeffAX * sx + coeffBX * sy + constCX
           fy = coeffAY * sx + coeffBY * sy + constCY
修改记录:
**************************************************************************************/
class POSE_TRANSFORM_C
{
public:
    POSE_TRANSFORM_C()
          : coeffAX_(0), coeffBX_(0), constCX_(0),
            coeffAY_(0), coeffBY_(0), constCY_(0)
    {
        tfPoseByMap_       = Eigen::Matrix4f::Identity();
        tfSonPoseByFather_ = Eigen::Matrix4f::Identity();
    }

    ~POSE_TRANSFORM_C(){}

    void SetTransRelationship(const COOR_2D_POSE_STRU &stSonPoseByMap, const COOR_2D_POSE_STRU &stFatherPoseByMap);
    void SetTransRelationship(const COOR_3D_POSE_STRU &stSonPoseByMap, const COOR_3D_POSE_STRU &stFatherPoseByMap);
    
    void TransformPointBySon2ByFather2D(const MyPoint &pointBySon, MyPoint &pointByFather);
    void TransformPointCloudBySon2ByFather2D(const MyCloud::Ptr pCloudBySon, MyCloud::Ptr pCloudByFather);

    void TransformPointCloudBySon2ByFather3D(const MyCloud::Ptr pCloudBySon, MyCloud::Ptr pCloudByFather);

    INT32 GetLocationMsgFromTF(COOR_3D_POSE_STRU &stPose);
    INT32 GetTransMatrixFromTF(Eigen::Matrix4f &tfPoseByMap);

private:
    float coeffAX_;
    float coeffBX_;
    float constCX_;
    float coeffAY_;
    float coeffBY_;
    float constCY_;

    Eigen::Matrix4f         tfSonPoseByFather_;
    Eigen::Matrix4f         tfPoseByMap_;
};


#endif

